var xu_endstop = 208
var y_endstop = 175
var endstop_offset = 5

; Safety margin for endstop approach
var safety_margin = 5




if !move.axes[0].homed || !move.axes[1].homed || !move.axes[2].homed || !move.axes[3].homed
    abort "Error: All axes must be homed before running this test."

M98 P"tfree3.g"                       ; Run the "tfree3.g" macro to free the tool
T0
M204 T2000                            ; Set acceleration for non-printing moves

; === SAFE POSITIONING BEFORE ENDSTOP CHECKS ===
; Move to safer center positions instead of near endstops
G90
G1 X-100 U100 Y0 F18000               ; Move to center-ish safe position

; Get axis limits from object model
var x_min = move.axes[0].min          ; X axis minimum limit
var x_max = move.axes[0].max          ; X axis maximum limit  
var y_min = move.axes[1].min          ; Y axis minimum limit
var y_max = move.axes[1].max          ; Y axis maximum limit
var u_min = move.axes[3].min          ; U axis minimum limit
var u_max = move.axes[3].max          ; U axis maximum limit

; Calculate safe travel distances with safety margin
var y_travel_distance = abs(var.y_min - move.axes[1].machinePosition) + var.safety_margin
var x_travel_distance = abs(var.x_min - move.axes[0].machinePosition) + var.safety_margin
var u_travel_distance = abs(var.u_max - move.axes[3].machinePosition) + var.safety_margin

;echo "Calculated safe travel distances:"
;echo "Y: " ^ {var.y_travel_distance} ^ " mm from current position to Y-min"
;echo "X: " ^ {var.x_travel_distance} ^ " mm from current position to X-min"  
;echo "U: " ^ {var.u_travel_distance} ^ " mm from current position to U-max"


;== Y-AXIS POSITION CHECK WITH SAFETY VERIFICATION ===

; Initialize variables to store the positions
var leftEndstopPosition = 0
var rightEndstopPosition = 0
var currentYPos = 0
var side = ""

; SAFETY CHECK: Move towards Y endstop with calculated safe distance
G91                                   ; Switch to relative positioning
G1 H4 Y{-var.y_travel_distance} F1800 ; Move towards Y endstop with safe calculated distance

; Verify endstop was actually triggered
if result == 0
    M98 P"0:/sys/led/fault.g"
    abort "ERROR: Y-axis endstop not triggered during safe approach. Possible endstop failure or mechanical issue."

G1 Y2 F12000                           ; Move away from endstop by 2 mm
G1 H4 Y-3 F140                         ; Precise approach

; Check which endstop was triggered and record position
M574 Y1 S1 P"io1.in"                  ; Configure right-hand side Y endstop on io1.in
M400
G4 P300

if sensors.endstops[1].triggered      ; Check if the right-hand side Y endstop is triggered
    set var.side = "right"            ; Right-hand side endstop (io1.in) is triggered
elif !sensors.endstops[1].triggered   ; If right-hand side not triggered, check left-hand side
    M574 Y1 S1 P"io2.in"              ; Configure left-hand side Y endstop on io2.in
    M400
    G4 P300
    if sensors.endstops[1].triggered  ; Check if the left-hand side Y endstop is triggered
        set var.side = "left"         ; Left-hand side endstop (io2.in) is triggered
    else
        M574 Y1 S1 P"io1.in+io2.in"  ; Check both endstops
        M400
        G4 P300
        if sensors.endstops[1].triggered
            set var.side = "both"     ; Both endstops triggered




; Record the first endstop position
M400
G4 P300
set var.currentYPos = move.axes[1].machinePosition
if var.side == "right"
    set var.rightEndstopPosition = var.currentYPos
    ;echo "Right Y endstop position: " ^ {var.rightEndstopPosition} ^ " mm"
elif var.side == "left" 
    set var.leftEndstopPosition = var.currentYPos
    ;echo "Left Y endstop position: " ^ {var.leftEndstopPosition} ^ " mm"
elif var.side == "both"
    set var.leftEndstopPosition = var.currentYPos
    set var.rightEndstopPosition = var.currentYPos
    ;echo "Both Y endstops triggered simultaneously. Please check wiring."

; Now check the other endstop by switching motor drivers
if var.side == "right"
  M584 Y0.4                           ; Switch to the other driver for the Y axis
  M574 Y1 S1 P"io2.in"                ; Configure the Y endstop on io2.in
  G1 H4 Y-10 F140                     ; Move towards left endstop and record position
  M400
  G4 P300
  set var.leftEndstopPosition = move.axes[1].machinePosition
  ;echo "Left Y endstop position: " ^ {var.leftEndstopPosition} ^ " mm"

if var.side == "left"
  M584 Y0.1                           ; Switch to the other driver for the Y axis  
  M574 Y1 S1 P"io1.in"                ; Configure the Y endstop on io1.in
  G1 H4 Y-10 F140                     ; Move towards right endstop and record position
  M400
  G4 P300
  set var.rightEndstopPosition = move.axes[1].machinePosition
  ;echo "Right Y endstop position: " ^ {var.rightEndstopPosition} ^ " mm"


; Restore Y-axis configuration
M584 Y0.1:0.4                         ; Use both drivers for the Y axis
M574 Y1 S1 P"io1.in+io2.in"           ; Configure endstops for both sides of the Y axis
M906 X1800 U1800 Y1800:1800 Z850 E600:600 I35 T10
M913 Y100                             ; Restore Y motor current to 100%

M400                                  ; Ensure all moves are completed

; === Y-AXIS DEVIATION CHECK ===
; Calculate Y-axis misalignment between left and right sides
var y_deviation = abs(var.leftEndstopPosition - var.rightEndstopPosition)

; Check if Y-axis deviation exceeds tolerance (similar to HomeY test)
if var.y_deviation >= 0.5
    M98 P"0:/sys/led/fault.g"
    var y_error_msg = "Y-axis misalignment detected!<br><br>"
    set var.y_error_msg = var.y_error_msg ^ "Left endstop: " ^ {var.leftEndstopPosition} ^ " mm<br>"
    set var.y_error_msg = var.y_error_msg ^ "Right endstop: " ^ {var.rightEndstopPosition} ^ " mm<br>"
    set var.y_error_msg = var.y_error_msg ^ "Deviation: " ^ {var.y_deviation} ^ " mm<br><br>"
    set var.y_error_msg = var.y_error_msg ^ "The Y-axis gantry is not square.<br>Adjust Y-endstops before continuing."
    M291 R"Y-Axis Alignment Error" P{var.y_error_msg} S2
    abort "ERROR: Y-axis deviation of " ^ {var.y_deviation} ^ " mm exceeds 0.5mm tolerance. Test aborted."

;echo "Y-axis alignment OK: " ^ {var.y_deviation} ^ " mm deviation"


 ; == X-AXIS POSITION CHECK WITH SAFETY VERIFICATION ===
G90                                   ; Switch to absolute positioning
G1 Y172 F18000                        ; Move to safe Y position
G1 X-100 U100 F18000                  ; Return to safer center position

; SAFETY CHECK: Move towards X endstop with calculated safe distance
G91                                   ; Switch to relative positioning
G1 H4 X{-var.x_travel_distance} F1800 ; Move towards X endstop with safe calculated distance

; Verify X endstop was actually triggered
if result == 0
    M98 P"0:/sys/led/fault.g"
    abort "ERROR: X-axis endstop not triggered during safe approach. Possible endstop failure or mechanical issue."

G1 X2 F1800                           ; Move away from endstop by 2 mm
G1 H4 X-3 F140                        ; Move slowly to ensure endstop is triggered

var xEndstopPosition = move.axes[0].machinePosition
;echo "X-axis endstop position: " ^ {var.xEndstopPosition} ^ " mm"

G1 H2 X5 F1800                        ; Move away from endstop by 5 mm

; == U-AXIS POSITION CHECK WITH SAFETY VERIFICATION ===

; SAFETY CHECK: Move towards U endstop with calculated safe distance
G91                                   ; Switch to relative positioning
G1 H4 U{var.u_travel_distance} F1800  ; Move towards U endstop with safe calculated distance

; Verify U endstop was actually triggered
if result == 0
    M98 P"0:/sys/led/fault.g"
    abort "ERROR: U-axis endstop not triggered during safe approach. Possible endstop failure or mechanical issue."

G1 U-2 F1800                          ; Move away from endstop by 2 mm
G1 H4 U3 F140                         ; Move slowly to ensure endstop is triggered

var uEndstopPosition = move.axes[3].machinePosition
;echo "U-axis endstop position: " ^ {var.uEndstopPosition} ^ " mm"

G1 H2 U-5 F1800                       ; Move away from endstop by 5 mm

M400                                  ; Ensure all moves are completed

; Create global variable with four axis positions array
if exists(global.axisPositions)
    set global.axisPositions = {var.xEndstopPosition, var.leftEndstopPosition, var.rightEndstopPosition, var.uEndstopPosition}
else
    global axisPositions = {var.xEndstopPosition, var.leftEndstopPosition, var.rightEndstopPosition, var.uEndstopPosition}